#ifndef _LINEAR_H
#define _LINEAR_H    1

#include "utils.h"

#define INSPIRE_FACTOR 0.25

/* 根据给定的笛卡尔空间速度,计算各个路径段的持续时间 */
void CalculateDuration(char *filename, double v_in_cartesian, double *td)
{
	FILE *rfp = fopen(filename, "r");
	int LineNum = wc_l(filename);

	double p[LineNum][3];
	char buf[100];
	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<3; j++)
		{
			fscanf(rfp, "%s", buf);
			p[i][j] = atof(buf);
		}
	}
	fclose(rfp);
	
	for (int i=0; i<LineNum-1; i++)
	{
		td[i] = Dist(p[i], p[i+1]) / v_in_cartesian;
		printf("%lf\n", td[i]);
	}
}

/* 单关节的多段带有过渡区段的直线路径的轨迹规划 */
void StraightPath(char *cmds, double v_in_cartesian, double a_in_joint)
{
	const int LineNum = wc_l("关节1轨迹.txt");

	/* 求各个路径段的持续时间 */
	double td[LineNum-1];
	CalculateDuration("关节1轨迹_笛卡尔空间.txt", v_in_cartesian, td);

	/* 读取中间点(关节1) */
	double theta[LineNum];

	FILE *rfp = fopen("关节1轨迹.txt", "r");
	char tmp[100];
	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(rfp, "%s", tmp);
			//读取关节1
			if (j == 0)
			{
				theta[i] = d2r(atof(tmp));
			}
		}
	}
	fclose(rfp);

	/* 计算直线区段速度 */
	double v[LineNum-1];
	for (int i=0; i<LineNum-1; i++)
	{
		v[i] = (theta[i+1] - theta[i]) / td[i];
	}

	/* 计算过渡区段的加速度和持续时间,每个中间点都有对应的过渡区段,起止点的过渡区段从0加速至v[0]或从v[n-1]减速至0 */
	double a[LineNum];
	double AcceTime[LineNum];
	for (int i=0; i<LineNum; i++)
	{
		if (i==0)
		{
			a[i] = sign(v[i]) * a_in_joint;
			AcceTime[i] = fabs(v[i] / a[i]);
		}
		else if (i==LineNum-1)
		{
			a[i] = sign(v[i-1]) * a_in_joint;
			AcceTime[i] = fabs(v[i-1] / a[i]);
		}
		else 
		{
			a[i] = sign(v[i] - v[i-1]) * a_in_joint;
			AcceTime[i] = fabs((v[i] - v[i-1]) / a[i]);
		}
	}

	/* 计算直线区段持续时间 */
	double LineTime[LineNum-1];
	for (int i=0; i<LineNum-1; i++)
	{
		LineTime[i] = td[i] - AcceTime[i] * 0.5 - AcceTime[i+1] * 0.5;

		//检查加速度是否满足要求
		assert(LineTime[i] >= 0);
	}

	/* 生成speedj命令程序 */
	sprintf(cmds, "def move():\n");

	/* 前n-1个点,t=AcceTime+LineTime */
	for (int i=0; i<LineNum-1; i++)
	{
		sprintf(cmds+strlen(cmds), "speedj([%lf,0,0,0,0,0],a=%lf,t=%lf)\n", v[i], a[i], AcceTime[i] + LineTime[i]);
	}
	/* 终止点,t=AcceTime */
	sprintf(cmds+strlen(cmds), "speedj([0,0,0,0,0,0],a=%lf,t=%lf)\n", a[LineNum-1], AcceTime[LineNum-1]);

	sprintf(cmds+strlen(cmds), "end\n");
}

/* 通过前后关节值和时间间隔计算过渡段加速度, 过渡段时间采用启发算法:t=0.25*(interval_back+interval_front) */
double CalcAcce(double theta_front, double theta, double theta_back, double interval_front, double interval_back)
{
	//前后间隔不能同时为0
	assert((interval_back!=0 || interval_front!=0));

	if (interval_front == 0)
	{
		return ((theta_back-theta)/interval_back - 0) / (INSPIRE_FACTOR*interval_back);
	} //始点
	else if (interval_back == 0)
	{
		return (0 - (theta-theta_front)/interval_front) / (INSPIRE_FACTOR*interval_front);
	} //末点
	else
	{
		return ((theta_back-theta)/interval_back - (theta-theta_front)/interval_front) / (INSPIRE_FACTOR*(interval_back+interval_front));
	} //中间点
}

void CheckDuration(double *td, double a_max)
{
	const int LineNum = wc_l("多轴的直线路径轨迹.txt");

	/* 读取轨迹 */
	double q[LineNum][6];

	FILE *rfp = fopen("多轴的直线路径轨迹.txt", "r");	
	char buf[100];
	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(rfp, "%s", buf);
			q[i][j] = atof(buf);
		}
	}
	fclose(rfp);
	
	/* 计算同一时刻下，各关节对应的加速度a,选取最大的a并与a_max进行比较 */
	double a[6];
	/* td(i-1)(i)和td(i)(i+1)的放缩系数 */
	double k;

	/* 起始点 */
	double tmp = 0;
	for (int i=0; i<6; i++)
	{
		a[i] = CalcAcce(0, q[0][i], q[1][i], 0, td[0]);

		if (fabs(a[i]) > tmp) {
			tmp = fabs(a[i]);
		}
	}

	if (tmp > a_max)
	{
		k = sqrt(tmp / a_max);
		td[0] = k * td[0];
	}

	/* 中间点 */
	for (int i=1; i<LineNum-1; i++)
	{
		tmp = 0;
		for (int j=0; j<6; j++)
		{
			a[j] = CalcAcce(q[i-1][j], q[i][j], q[i+1][j], td[i-1], td[i]);

			if (fabs(a[j]) > tmp) {
				tmp = fabs(a[j]);
			}
		}
		
		if (tmp > a_max)
		{
			k = sqrt(tmp / a_max);
			td[i-1] = k * td[i-1];
			td[i] = k * td[i];
		}
	}

	/* 终止点 */
	tmp = 0;
	for (int i=0; i<6; i++)
	{
		a[i] = CalcAcce(q[LineNum-2][i], q[LineNum-1][i], 0, td[LineNum-2], 0);

		if (fabs(a[i]) > tmp) {
			tmp = fabs(a[i]);
		}
	}

	if (tmp > a_max)
	{
		k = sqrt(tmp / a_max);
		td[LineNum-2] = k * td[LineNum-2];
	}
}

void StraightPath_MultAxis(char *cmds, double v_in_cartesian, double a_max)
{
	const int LineNum = wc_l("多轴的直线路径轨迹.txt");

	/* 求各个路径段的持续时间 */
	double td[LineNum-1];
	CalculateDuration("多轴的直线路径轨迹_笛卡尔空间.txt", v_in_cartesian, td);

	/* 检查默认加速度是否可行 */ 
	CheckDuration(td, a_max);

	/* 读取轨迹 */
	double q[LineNum][6];

	FILE *rfp = fopen("多轴的直线路径轨迹.txt", "r");	
	char buf[100];
	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(rfp, "%s", buf);
			q[i][j] = atof(buf);
		}
	}
	fclose(rfp);

	/* 计算直线区段速度 */
	double v[LineNum-1][6];
	for (int i=0; i<LineNum-1; i++)
	{
		for (int j=0; j<6; j++)
		{
			v[i][j] = (q[i+1][j] - q[i][j]) / td[i];
		}
	}
	
	/* 计算主轴加速度 */
	double a[LineNum];
	/* 记录每次加速的leading axis */
	int lead_axis[LineNum];
	double vd_temp[6];//当前点下各轴加速度
	double tmp;//当前点下各轴加速度中的最大值，作为主轴加速度

	/* 起始点 */
	tmp = 0;
	for (int i=0; i<6; i++)
	{
		vd_temp[i] = CalcAcce(0, q[0][i], q[1][i], 0, td[0]);

		if (fabs(vd_temp[i]) > tmp)
		{
			a[0] = vd_temp[i];
			lead_axis[0] = i;

			tmp = fabs(vd_temp[i]);
		}
	}

	assert(tmp <= a_max+0.1); //+0.1以防止d_temp略大于a_max而报错
	
	/* 中间点 */
	for (int i=1; i<LineNum-1; i++)
	{
		tmp = 0;
		for (int j=0; j<6; j++)
		{
			vd_temp[j] = CalcAcce(q[i-1][j], q[i][j], q[i+1][j], td[i-1], td[i]);

			if (fabs(vd_temp[j]) > tmp)
			{
				a[i] = vd_temp[j];
				lead_axis[i] = j;

				tmp = fabs(vd_temp[j]);
			}
		}
		
		assert(tmp <= a_max + 0.1);
	}

	/* 终止点 */
	tmp = 0;
	for (int i=0; i<6; i++)
	{
		vd_temp[i] = CalcAcce(q[LineNum-2][i], q[LineNum-1][i], 0, td[LineNum-2], 0);

		if (fabs(vd_temp[i]) > tmp)
		{
			a[LineNum-1] = vd_temp[i];
			lead_axis[LineNum-1] = i;

			tmp = fabs(vd_temp[i]);
		}
	}
	
	assert(tmp <= a_max + 0.1);
	
	/*计算过渡段时间:ti=|△v/ai|
	// 每个点都有一对应过渡段。特别的，对于始点和末点，其过渡段负责从0加速至v01 以及 从v(n-1)(n)减速至0
	// 对于多轴运动来讲，各轴在同一时刻点的过渡段时间是相等的，因此我们可以计算出任意关节对应的过渡段时间，来作为此时刻点对应的过渡段时间。此中，我们计算主轴对应的过渡段时间。\
	// 进一步的，由于过渡段时间相等，因此直线段时间也相等，也只需求关节1的直线段时间。*/
	double AcceTime[LineNum];
	for (int i=0; i<LineNum; i++)
	{
		if (i == 0)
		{
			AcceTime[i] = fabs(v[i][lead_axis[i]] / a[i]);
		}
		else if (i == LineNum-1)
		{
			AcceTime[i] = fabs(v[i-1][lead_axis[i]] / a[i]);
		}
		else
		{
			if (a[i] == 0)
			{
				AcceTime[i] = 0.25 * (td[i] + td[i - 1]);
			}
			else
			{
				AcceTime[i] = fabs((v[i][lead_axis[i]] - v[i-1][lead_axis[i]]) / a[i]);
			}
		}
	}

	/* 计算直线区段持续时间 */
	double LineTime[LineNum-1];
	for (int i=0; i<LineNum-1; i++)
	{
		LineTime[i] = 2 * INSPIRE_FACTOR * td[i];
	}

	/* 生成speedj命令程序 */
	//每个点对应一个speedj命令，如点0的speedj: 以加速度a[0]加速至v[0][...]，然后匀速运动，整个过程的持续时间为t = t[0] + LineTime[0]
	//特别的，对于末点n = LineNum-1，其对应的命令为stopj: stopj的加速度为a = a[LineNum-1]
	sprintf(cmds, "def move():\n");
	double qd[6] = {0};
	for (int i=0; i<LineNum-1; i++)
	{
		for (int j=0; j<6; j++)
		{
			qd[j] = v[i][j];
		}

		sprintf(cmds+strlen(cmds), "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,t=%lf)\n", qd[0],qd[1],qd[2],qd[3],qd[4],qd[5], a[i], AcceTime[i] + LineTime[i]);
	}
	for (int j=0; j<6; j++)
	{
		qd[j] = v[LineNum-1][j];
	}
	sprintf(cmds+strlen(cmds), "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,t=%lf)\n", qd[0],qd[1],qd[2],qd[3],qd[4],qd[5], a[LineNum-1], AcceTime[LineNum-1]);
	sprintf(cmds+strlen(cmds), "end\n");
}

#endif /* <linear.h> included.  */
